//
// Created by bourne on 2024/4/20.
//

#ifndef SRC_GRAPH_NODE_H
#define SRC_GRAPH_NODE_H

#include <vector>
#include <unordered_map>
#include <queue>
#include <list>
#include <memory>
#include <iostream>
#include <math.h>
#include <algorithm>
#include <Eigen/Eigen>

using std::list;
using std::queue;
using std::shared_ptr;
using std::unique_ptr;
using std::unordered_map;
using std::vector;
using std::cout;
using Eigen::Vector3d;
using Eigen::Vector3i;

class RayCaster;

namespace fast_planner {

// Basic noded type containing only general artributes required by graph search
// 基本节点类型，仅包含图搜索所需的通用属性
class BaseNode {
public:
  typedef shared_ptr<BaseNode> Ptr;// 共享指针类型定义，用于智能地管理BaseNode对象的生命周期
  // 构造函数，初始化节点的g值（从起始节点到当前节点的代价）为一个非常大的数，
  // 表示初始时节点未被探索，closed标志位初始化为false，表示节点未被关闭（即未被访问）
  BaseNode() {
    g_value_ = 1000000;
    closed_ = false;
  }
  ~BaseNode() {
  }

  // 打印节点信息的虚函数，可以在派生类中重写以打印更多信息
  virtual void print() {
    std::cout << "Base node" << std::endl;
  }

  int id_; // 节点的ID，用于唯一标识一个节点
  bool closed_; // 标志位，表示节点是否已经被访问
  double g_value_; // 从起始节点到当前节点的代价，用于A*搜索算法中的g值
};


// Astar类和SDFMap类的前向声明，表明这些类在其他地方定义
class Astar;
class SDFMap;

// Node type for viewpoint refinement
// 视点细化的节点类型，继承自BaseNode类
class ViewNode : public BaseNode {
public:
  // 共享指针类型定义，用于智能地管理ViewNode对象的生命周期
  typedef shared_ptr<ViewNode> Ptr;

  // 构造函数，接受位置和偏航角作为参数
  ViewNode(const Vector3d& p, const double& y);
  ViewNode() {} // 默认构造函数
  ~ViewNode() {} // 析构函数

  // 重写BaseNode类的print函数，打印视点节点的偏航角
  virtual void print() {
    std::cout << "View node" << yaw_ << std::endl;
  }

  // 打印邻居节点信息的函数
  void printNeighbors() {
    for (auto v : neighbors_) v->print();
  }

  // 计算当前节点到另一个节点的代价
  double costTo(const ViewNode::Ptr& node);

  // 计算两个节点间的代价，考虑位置和偏航角
  static double computeCost(const Vector3d& p1, const Vector3d& p2, const double& y1,
                            const double& y2, const Vector3d& v1, const double& yd1, vector<Vector3d>& path);

  // Coarse to fine path searching
  // 粗到细的路径搜索，尝试找到两个点之间的路径
  static double searchPath(const Vector3d& p1, const Vector3d& p2, vector<Vector3d>& path);

  // Data
  vector<ViewNode::Ptr> neighbors_; // 邻居节点列表
  ViewNode::Ptr parent_; // 父节点
  Vector3d pos_, vel_; // 节点的位置和速度
  double yaw_, yaw_dot_; // 偏航角和偏航角速度

  // 节点间共享的参数
  // Parameters shared among nodes
  static double vm_, am_, yd_, ydd_, w_dir_; // 速度、加速度、偏航角速度、偏航角加速度和方向权重
  // Astar搜索算法、射线投射器和地图服务的共享指针
  static shared_ptr<Astar> astar_;
  static shared_ptr<RayCaster> caster_;
  static shared_ptr<SDFMap> map_;
};
}
#endif //SRC_GRAPH_NODE_H
